/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#pragma once
#include "KF.h"
namespace hitcrt {
/**
 * Copyright (C) 2022, HITCRT_VISION, all rights reserved.
 * @description: 辛格模型卡尔曼滤波，没用到当前统计
 * @author: WWW
 * @mail: 1922039181@qq.com
 */
class KFS : public KF {
  public:
    KFS(const int measureDim, const int stateDim) : KF(measureDim, stateDim) {
        m_stateSaved = (cv::Mat::zeros(stateDim, 1, CV_64F));
    }
    ~KFS() = default;
    typedef enum Choice {
        PRIOR, //先验状态
        POST   //后验状态
    } StateChoice;

    virtual void init(const std::vector<boost::any> &param) override;
    virtual void setParam(const std::vector<boost::any> &param) override;
    void predictWithFixedVA(); //速度和加速度不更新的滤波，用于轨迹断裂

    void statePredict();                                     //无测量预测
    void reset(StateChoice choice, const cv::Mat &resetMat); //重置滤波器
    void saveState(StateChoice choice);                      //保存状态0存先验，1存后验
    void setVandA(StateChoice choice);                       //载入保存的状态

  private:
    double R0;
    double alpha;
    double sigma2;
    cv::Mat m_stateSaved;
    //暂未用到
    double m_acceleration;
    double m_accelerationMax;
    cv::Mat m_controlledMatrix;

    void setQ();
    void setCon();
    void setTransition();
};
} // namespace hitcrt